#include "sortedVectorByXY.h"
#include <iostream>

using namespace std;


//Constructor
sortedVectorByXY::sortedVectorByXY()
{
	vectorNodes = new vector<Node*>(0);
}


//Add the node to the vectorNodes to a position in which the vector is sorted by X and Y attributes
void sortedVectorByXY::addNode(Node* node)
{
	int max = vectorNodes->size();
	int min = 0;
	int solution = 0;
	vector<Node*>::iterator it = vectorNodes->begin();
	while(min < max)
	{
		if(min + 1 == max)
		{
			if(vectorNodes->at(min)->greaterThanByXY(node->getX(), node->getY())) solution = min;
			else if(max == vectorNodes->size() || vectorNodes->at(max)->greaterThanByXY(node->getX(), node->getY())) solution = max;
			else solution = max + 1;
			break;
		}
		int middle = (min + max) / 2;
		if(vectorNodes->at(middle)->lessThanByXY(node->getX(), node->getY()))
		{
			min = middle;
		}
		else if(vectorNodes->at(middle)->greaterThanByXY(node->getX(), node->getY()))
		{
			max = middle;
		}
	}
	if(vectorNodes->size() > 0)
	{
		if(solution == vectorNodes->size()) vectorNodes->push_back(node);
		else vectorNodes->insert(it + solution, node);
	}
	else
	{
		vectorNodes->push_back(node);
	}
}


//Returns true if the vector is empty
bool sortedVectorByXY::empty()
{
	return vectorNodes->empty();
}


//Removes a node from the vector using its iterator
void sortedVectorByXY::remove(vector<Node*>::iterator it)
{
	vectorNodes->erase(it);
}


//Allocates memory for the vector
void sortedVectorByXY::allocate(int width, int height)
{
	vectorNodes->reserve(width*height);
}


//Frees every node in the vector
void sortedVectorByXY::freeNodes()
{
	for(vector<Node*>::iterator it = vectorNodes->begin(); it != vectorNodes->end(); ++it)
	{
		delete *it;
	}

	delete vectorNodes;
}


//Returns the node with lowest score from the vector
Node* sortedVectorByXY::selectNodeLowestScore()
{
	//Initialise the lowest node with the first node in the vector
	Node* lowestNode = vectorNodes->front();

	//Initialise the iterator that will point to the lowest node
	vector<Node*>::iterator itLowestNode = vectorNodes->begin();

	//Scan every node in the vector and save in lowestNode the node with lowest score
	for (vector<Node*>::iterator it = vectorNodes->begin(); it != vectorNodes->end(); ++it)
	{
		if( (*it)->getF() < lowestNode->getF())
		{
			lowestNode = *it;
			itLowestNode = it;
		}
	}

	//Remove the lowest node from the vector
	vectorNodes->erase(itLowestNode);
	
	return lowestNode;
}


//Look for a node in the vectorNodes.
//If found, return a pointer to the node and its iterator
std::pair<std::vector<Node*>::iterator, Node*> sortedVectorByXY::findNode(int x, int y)
{
	std::pair<std::vector<Node*>::iterator, Node*> p;
	int max = vectorNodes->size() - 1;
	int min = 0;
	vector<Node*>::iterator it = vectorNodes->begin();
	while(min <= max)
	{
		int middle = (min + max) / 2;
		if(vectorNodes->at(middle)->lessThanByXY(x, y))
		{
			min = middle + 1;
		}
		else if(vectorNodes->at(middle)->greaterThanByXY(x, y))
		{
			max = middle - 1;
		}
		else
		{
			p.first = it + middle;
			p.second = *(it + middle);
			return p;
		}
	}

	p.first = it;
	p.second = 0;
	return p;
}


//Remove the node from the vector and add it to it again keeping the vector sorted
void sortedVectorByXY::modifyNode(std::vector<Node*>::iterator it)
{
	Node* node = *it;

	//Remove the nodes from the vectors
	vectorNodes->erase(it);

	//Add the node to the vectors
	addNode(node);
}